#include <unordered_map>
#include <functional>

#include "CameraParams.h"
#include "MvCameraControl.h"
#include "MvErrorDefine.h"
#include "MvObsoleteInterfaces.h"
#include "camdrv.hpp"

//      💩
//     💩💩
//   💩💩💩💩
//  💩💩💩💩💩
// 💩💩💩💩💩💩


namespace rm_camdrv {

void CamdrvNode::declareParameters() {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  MVCC_FLOATVALUE f_value;
  MVCC_INTVALUE i_value;
  MVCC_ENUMVALUE e_value;
  param_desc.integer_range.resize(1);
  param_desc.integer_range[0].step = 1;
  param_desc.floating_point_range.resize(1);

  // Currently, all our cameras use the same model, so all parameters are based on one camera.
  auto handle = default_camera_handle_;

  // Width
  param_desc.description = "Width of the camera image";
  MV_CC_SetAOIoffsetX(handle, 0); // Reset offset before setting width
  MV_CC_GetWidth(handle, &i_value);
  param_desc.integer_range[0].from_value = i_value.nMin;
  param_desc.integer_range[0].to_value = i_value.nMax;
  int width = this->declare_parameter<int>("Width", i_value.nCurValue, param_desc);
  MV_CC_SetWidth(handle, width);
  MV_CC_SetAOIoffsetX(handle, (i_value.nMax - i_value.nCurValue) / 2);

  // Height
  param_desc.description = "Height of the camera image";
  MV_CC_SetAOIoffsetY(handle, 0); // Reset offset before setting height
  MV_CC_GetHeight(handle, &i_value);
  param_desc.integer_range[0].from_value = i_value.nMin;
  param_desc.integer_range[0].to_value = i_value.nMax;
  int height = this->declare_parameter<int>("Height", i_value.nCurValue, param_desc);
  MV_CC_SetHeight(handle, height);
  MV_CC_SetAOIoffsetY(handle, (i_value.nMax - i_value.nCurValue) / 2);

  // Exposure time
  param_desc.description = "Exposure time in microseconds";
  MV_CC_GetExposureTime(handle, &f_value);
  param_desc.floating_point_range[0].from_value = f_value.fMin;
  param_desc.floating_point_range[0].to_value = 15000;
  double exposure_time =
      this->declare_parameter<double>("ExposureTime", f_value.fCurValue, param_desc);
  MV_CC_SetExposureTime(handle, exposure_time);

  // Gain
  param_desc.description = "Gain";
  MV_CC_GetGain(handle, &f_value);
  param_desc.floating_point_range[0].from_value = f_value.fMin;
  param_desc.floating_point_range[0].to_value = f_value.fMax;
  double gain = this->declare_parameter<double>("Gain", f_value.fCurValue, param_desc);
  MV_CC_SetGain(handle, gain);

  // Balance Ratio Red
  param_desc.description = "Balance Ratio Red";
  MV_CC_GetBalanceRatioRed(handle, &i_value);
  param_desc.integer_range[0].from_value = i_value.nMin;
  param_desc.integer_range[0].to_value = i_value.nMax;
  int balance_ratio_red = this->declare_parameter<int>("BalanceRatioRed", i_value.nCurValue, param_desc);
  MV_CC_SetBalanceRatioRed(handle, balance_ratio_red);

  // Balance Ratio Green
  param_desc.description = "Balance Ratio Green";
  MV_CC_GetBalanceRatioGreen(handle, &i_value);
  param_desc.integer_range[0].from_value = i_value.nMin;
  param_desc.integer_range[0].to_value = i_value.nMax;
  int balance_ratio_green = this->declare_parameter<int>("BalanceRatioGreen", i_value.nCurValue, param_desc);
  MV_CC_SetBalanceRatioGreen(handle, balance_ratio_green);

  // Balance Ratio Blue
  param_desc.description = "Balance Ratio Blue";
  MV_CC_GetBalanceRatioBlue(handle, &i_value);
  param_desc.integer_range[0].from_value = i_value.nMin;
  param_desc.integer_range[0].to_value = i_value.nMax;
  int balance_ratio_blue = this->declare_parameter<int>("BalanceRatioBlue", i_value.nCurValue, param_desc);
  MV_CC_SetBalanceRatioBlue(handle, balance_ratio_blue);

  // Auto White Balance (0: OFF; 1: CONTINUOUS; 2: ONCE)
  param_desc.description = "Auto White Balance (0: OFF; 1: CONTINUOUS; 2: ONCE)";
  MV_CC_GetBalanceWhiteAuto(handle, &e_value);
  param_desc.integer_range[0].from_value = 0;
  param_desc.integer_range[0].to_value = 2;
  int auto_white_balance = this->declare_parameter<int>("AutoWhiteBalance", e_value.nCurValue, param_desc);
  MV_CC_SetBalanceWhiteAuto(handle, auto_white_balance);

  // Use native RGB8 format if supported (default: true)
  param_desc.description = "Use native RGB8 format instead of bayer conversion";
  bool use_native_rgb8 = this->declare_parameter<bool>("UseNativeRGB8", true, param_desc);
  prefer_native_rgb8_ = use_native_rgb8;

  // Mode parameters
  for (auto& [mode, params] : mode_params_) {
    params->width = this->declare_parameter<int>(mode + ".Width", width);
    params->height = this->declare_parameter<int>(mode + ".Height", height);
    params->exposure_time = this->declare_parameter<double>(mode + ".ExposureTime", exposure_time);
    params->gain = this->declare_parameter<double>(mode + ".Gain", gain);
    params->auto_white_balance = this->declare_parameter<int>(mode + ".AutoWhiteBalance", auto_white_balance);
    params->balance_ratio_red = this->declare_parameter<int>(mode + ".BalanceRatioRed", balance_ratio_red);
    params->balance_ratio_green = this->declare_parameter<int>(mode + ".BalanceRatioGreen", balance_ratio_green);
    params->balance_ratio_blue = this->declare_parameter<int>(mode + ".BalanceRatioBlue", balance_ratio_blue);
  }
  RCLCPP_INFO(this->get_logger(), "Parameters declared successfully!");
}

rcl_interfaces::msg::SetParametersResult CamdrvNode::parametersCallback(
    const std::vector<rclcpp::Parameter> &parameters) {
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  if (mode_params_.find(current_mode_) == mode_params_.end()) {
    result.successful = false;
    result.reason = "Current mode not found: " + current_mode_;
    RCLCPP_ERROR(this->get_logger(), "Current mode not found: %s", current_mode_.c_str());
    return result;
  }
  auto handle = mode_params_.at(current_mode_)->camera_handle;
  for (const auto &param : parameters) {
    using ParamHandler = std::function<int(const rclcpp::Parameter&)>;
    std::unordered_map<std::string, ParamHandler> param_handlers = {
      {"Width",             [&](const auto& p) {
        MV_CC_SetAOIoffsetX(handle, 0); // Reset offset before setting width
        int status1 = MV_CC_SetWidth(handle, p.as_int());
        MVCC_INTVALUE width_info;
        MV_CC_GetWidth(handle, &width_info);
        int status2 = MV_CC_SetAOIoffsetX(handle, (width_info.nMax - width_info.nCurValue) / 2);
        return status1 | status2;
      }},
      {"Height",            [&](const auto& p) {
        MV_CC_SetAOIoffsetY(handle, 0); // Reset offset before setting height
        int status1 = MV_CC_SetHeight(handle, p.as_int());
        MVCC_INTVALUE height_info;
        MV_CC_GetHeight(handle, &height_info);
        int status2 = MV_CC_SetAOIoffsetY(handle, (height_info.nMax - height_info.nCurValue) / 2);
        return status1 | status2;
      }},
      {"ExposureTime",      [&](const auto& p) { return MV_CC_SetExposureTime(handle, p.as_double()); }},
      {"Gain",              [&](const auto& p) { return MV_CC_SetGain(handle, p.as_double()); }},
      {"AutoWhiteBalance",  [&](const auto& p) { return MV_CC_SetBalanceWhiteAuto(handle, p.as_int()); }},
      {"BalanceRatioRed",   [&](const auto& p) { return MV_CC_SetBalanceRatioRed(handle, p.as_int()); }},
      {"BalanceRatioGreen", [&](const auto& p) { return MV_CC_SetBalanceRatioGreen(handle, p.as_int()); }},
      {"BalanceRatioBlue",  [&](const auto& p) { return MV_CC_SetBalanceRatioBlue(handle, p.as_int()); }}
    };
    
    int status = MV_E_UNKNOW;
    
    auto it = param_handlers.find(param.get_name());
    if (it != param_handlers.end()) {
      status = it->second(param);
    } else {
      result.successful = false;
      result.reason = "Unknown parameter: " + param.get_name();
    }
    
    if (MV_OK != status) {
      result.successful = false;
      result.reason = "Failed to set " + param.get_name() + ", status = " + std::to_string(status);
      RCLCPP_ERROR(this->get_logger(), "STATUS ERROR: 0x%x", status);
    } else {
      if (param.get_type() == 2) {
        RCLCPP_INFO(this->get_logger(), "%s: %ld", param.get_name().c_str(), param.as_int());
      } else if (param.get_type() == 3) {
        RCLCPP_INFO(this->get_logger(), "%s: %.1lf", param.get_name().c_str(), param.as_double());
      } else {
        RCLCPP_ERROR(this->get_logger(), "PARAMETER TYPE ERROR!");
      }
    }
  }
  return result;
}

}